#include <jtag/interface.h>
#include <jtag/jtag_tap.h>

int interface_jtag::interface_jtag_add_ir_scan(struct jtag_tap *active,
		const struct scan_field *in_fields, tap_state_t state)
{
	size_t num_taps = 1;

	struct jtag_command *cmd = (struct jtag_command *)cmd_queue_alloc(sizeof(struct jtag_command));
	struct scan_command *scan = (struct scan_command *)cmd_queue_alloc(sizeof(struct scan_command));
	struct scan_field *out_fields = (struct scan_field *)cmd_queue_alloc(num_taps  * sizeof(struct scan_field));

	jtag_queue_command(cmd);

	cmd->type = JTAG_SCAN;
	cmd->cmd.scan = scan;

	scan->ir_scan = true;
	scan->num_fields = num_taps;	/* one field per device */
	scan->fields = out_fields;
	scan->end_state = state;

	struct scan_field *field = out_fields;	/* keep track where we insert data */

    jtag_scan_field_clone(field, in_fields);
	/* update device information */
	//buf_cpy(field->out_value, tap->cur_instr, tap->ir_length);

	return ERROR_OK;
}

int interface_jtag::interface_jtag_add_dr_scan(jtag_tap *active, int in_num_fields,
		const struct scan_field *in_fields, tap_state_t state)
{
    size_t bypass_devices = 0;

    struct jtag_command *cmd = (struct jtag_command *)cmd_queue_alloc(sizeof(struct jtag_command));
	struct scan_command *scan = (struct scan_command *)cmd_queue_alloc(sizeof(struct scan_command));
	struct scan_field *out_fields = (struct scan_field *)cmd_queue_alloc((in_num_fields + bypass_devices) * sizeof(struct scan_field));

    if (!cmd || !scan || !out_fields)
    {
        return -1;
    }

	jtag_queue_command(cmd);

	cmd->type = JTAG_SCAN;
	cmd->cmd.scan = scan;

	scan->ir_scan = false;
	scan->num_fields = in_num_fields + bypass_devices;
	scan->fields = out_fields;
	scan->end_state = state;

    struct scan_field *field = out_fields;

    jtag_scan_field_clone(field, in_fields);

    return ERROR_OK;
}

int interface_jtag::interface_jtag_add_plain_dr_scan(int num_bits, const uint8_t *out_bits, uint8_t *in_bits, tap_state_t state)
{
	return jtag_add_plain_scan(num_bits, out_bits, in_bits, state, false);
}

int interface_jtag::interface_jtag_add_tlr(void)
{
	tap_state_t state = TAP_RESET;

	/* allocate memory for a new list member */
	struct jtag_command *cmd = (struct jtag_command *)cmd_queue_alloc(sizeof(struct jtag_command));

	jtag_queue_command(cmd);

	cmd->type = JTAG_TLR_RESET;

	cmd->cmd.statemove = (struct statemove_command *)cmd_queue_alloc(sizeof(struct statemove_command));
	cmd->cmd.statemove->end_state = state;

	return ERROR_OK;
}

int interface_jtag::interface_jtag_add_runtest(int num_cycles, tap_state_t state)
{
	/* allocate memory for a new list member */
	struct jtag_command *cmd = (struct jtag_command *)cmd_queue_alloc(sizeof(struct jtag_command));

	jtag_queue_command(cmd);

	cmd->type = JTAG_RUNTEST;

	cmd->cmd.runtest = (struct runtest_command*)cmd_queue_alloc(sizeof(struct runtest_command));
	cmd->cmd.runtest->num_cycles = num_cycles;
	cmd->cmd.runtest->end_state = state;

	return ERROR_OK;
}

int interface_jtag::interface_jtag_add_reset(int req_trst, int req_srst)
{
	/* allocate memory for a new list member */
	struct jtag_command *cmd = (struct jtag_command *)cmd_queue_alloc(sizeof(struct jtag_command));

	jtag_queue_command(cmd);

	cmd->type = JTAG_RESET;

	cmd->cmd.reset = (struct reset_command*)cmd_queue_alloc(sizeof(struct reset_command));
	cmd->cmd.reset->trst = req_trst;
	cmd->cmd.reset->srst = req_srst;

	return ERROR_OK;
}

/******************************************************/
int interface_jtag::jtag_add_plain_scan(int num_bits, const uint8_t *out_bits,
		uint8_t *in_bits, tap_state_t state, bool ir_scan)
{
	struct jtag_command *cmd = (struct jtag_command *)cmd_queue_alloc(sizeof(struct jtag_command));
	struct scan_command *scan = (struct scan_command *)cmd_queue_alloc(sizeof(struct scan_command));
	struct scan_field *out_fields = (struct scan_field *)cmd_queue_alloc(sizeof(struct scan_field));

	jtag_queue_command(cmd);

	cmd->type = JTAG_SCAN;
	cmd->cmd.scan = scan;

	scan->ir_scan = ir_scan;
	scan->num_fields = 1;
	scan->fields = out_fields;
	scan->end_state = state;

	out_fields->num_bits = num_bits;
	out_fields->out_value = (const uint8_t *)buf_cpy(out_bits, cmd_queue_alloc(DIV_ROUND_UP(num_bits, 8)), num_bits);
	out_fields->in_value = in_bits;

	return ERROR_OK;
}
